// 24.6.17 Mzz 
// 逐帧保存大恒相机的图像，保存为.ppm格式。
#include "GxIAPI.h"
#include "DxImageProc.h"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"    // 字符串消息类型
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <thread>
#include <vector>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <unistd.h>
#include <time.h>
typedef unsigned char BYTE;
using namespace cv;
using namespace std;
#define GX_VERIFY(emStatus)
#define GX_VERIFY_EXIT(emStatus)
#define ACQ_TRANSFER_SIZE       (64 * 1024)
#define ACQ_BUFFER_NUM          5
#define ACQ_TRANSFER_NUMBER_URB 64
#define FILE_NAME_LEN           100 
unsigned char* g_pRGBImageBuf = NULL;
unsigned char* g_pRaw8Image = NULL;
#define PIXFMT_CVT_SUCCESS          0 
#define PIXFMT_CVT_FAIL             -1
int64_t g_nPayloadSize = 0;
#define CAMERA_NUM		2
int all_frame = 0;
int accept_frame = 0;
int lose_frame = 0;
std::string* return_value = nullptr;
int64_t g_i64ColorFilter = GX_COLOR_FILTER_NONE;
void GetErrorString(GX_STATUS);
pthread_t g_nAcquisitonThreadID = 0; 
std::string SavePPMFile(uint32_t, uint32_t);
int PixelFormatConvert(PGX_FRAME_BUFFER);
bool g_bSavePPMImage = false;
GX_DEV_HANDLE g_hDevice[CAMERA_NUM] = {NULL};              ///< Device handle 
bool g_bAcquisitionFlag = false;
extern char camera_type;
void *ProcGetImage(void*);
void UnPreForAcquisition();
class PublisherNode : public rclcpp::Node
{
    public:
        PublisherNode()
        : Node("save_image") 
        {
            publisher_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);       
            //Initialize libary
            emStatus = GXInitLib(); 
            if(emStatus != GX_STATUS_SUCCESS)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open camera stream.");
            }            
            //Get device enumerated number
            emStatus = GXUpdateDeviceList(&ui32DeviceNum, 1000);
            if(emStatus != GX_STATUS_SUCCESS)
            { 
                RCLCPP_ERROR(this->get_logger(), "Failed to open camera stream.");
            }
            if(ui32DeviceNum <= 0)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open camera stream.");
            }
            emStatus = GXOpenDeviceByIndex(1, &g_hDevice[0]);
            if(emStatus != GX_STATUS_SUCCESS)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open camera stream.");           
            }

            emStatus = GXGetStringLength(g_hDevice[0], GX_STRING_DEVICE_VENDOR_NAME, &nSize);
            GX_VERIFY_EXIT(emStatus);
            //Alloc memory for Vendor name
            char *pszVendorName = new char[nSize];
            //Get Vendor name
            emStatus = GXGetString(g_hDevice[0], GX_STRING_DEVICE_VENDOR_NAME, pszVendorName, &nSize);
            if (emStatus != GX_STATUS_SUCCESS)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open camera stream.");
            }
            delete[] pszVendorName;
            pszVendorName = NULL;           
            //Get string length of Model name
            emStatus = GXGetStringLength(g_hDevice[0], GX_STRING_DEVICE_MODEL_NAME, &nSize);
            GX_VERIFY_EXIT(emStatus);
            //Alloc memory for Model name
            char *pszModelName = new char[nSize];
            //Get Model name
            emStatus = GXGetString(g_hDevice[0], GX_STRING_DEVICE_MODEL_NAME, pszModelName, &nSize);
            if (emStatus != GX_STATUS_SUCCESS)
            {
                delete[] pszModelName;
                pszModelName = NULL;
                GX_VERIFY_EXIT(emStatus);
            }
            camera_type = pszModelName[5]; 
            printf("<Model Name : %s>\n", pszModelName);
            //Release memory for Model name
            delete[] pszModelName;
            pszModelName = NULL;

            printf("***********************************************\n");
            printf("Start collecting image\n"); 
            printf("***********************************************\n");        
            //Get the type of Bayer conversion. whether is a color camera.
            emStatus = GXIsImplemented(g_hDevice[0], GX_ENUM_PIXEL_COLOR_FILTER, &g_bColorFilter);
            GX_VERIFY_EXIT(emStatus);

            //This app only support color cameras
            if (!g_bColorFilter)
            {
                printf("<This app only support color cameras! App Exit!>\n");
                GXCloseDevice(g_hDevice[0]);
                g_hDevice[0] = NULL;
                GXCloseLib();
            }
            else
            {
                emStatus = GXGetEnum(g_hDevice[0], GX_ENUM_PIXEL_COLOR_FILTER, &g_i64ColorFilter);
                GX_VERIFY_EXIT(emStatus);
            }
            
            emStatus = GXGetInt(g_hDevice[0], GX_INT_PAYLOAD_SIZE, &g_nPayloadSize);
            GX_VERIFY(emStatus);
        //Set acquisition mode
            emStatus = GXSetEnum(g_hDevice[0], GX_ENUM_ACQUISITION_MODE, GX_ACQ_MODE_CONTINUOUS);
            GX_VERIFY_EXIT(emStatus);

            //Set trigger mode
            emStatus = GXSetEnum(g_hDevice[0], GX_ENUM_TRIGGER_MODE, GX_TRIGGER_MODE_OFF);
            GX_VERIFY_EXIT(emStatus);

            //Set buffer quantity of acquisition queue
            uint64_t nBufferNum = ACQ_BUFFER_NUM;
            emStatus = GXSetAcqusitionBufferNumber(g_hDevice[0], nBufferNum);
            GX_VERIFY_EXIT(emStatus);

            bool bStreamTransferSize = false;
            emStatus = GXIsImplemented(g_hDevice[0], GX_DS_INT_STREAM_TRANSFER_SIZE, &bStreamTransferSize);
            GX_VERIFY_EXIT(emStatus);

            if(bStreamTransferSize)
            {
                //Set size of data transfer block
                emStatus = GXSetInt(g_hDevice[0], GX_DS_INT_STREAM_TRANSFER_SIZE, ACQ_TRANSFER_SIZE);
                GX_VERIFY_EXIT(emStatus);
            }

            bool bStreamTransferNumberUrb = false;
            emStatus = GXIsImplemented(g_hDevice[0], GX_DS_INT_STREAM_TRANSFER_NUMBER_URB, &bStreamTransferNumberUrb);
            GX_VERIFY_EXIT(emStatus);

            if(bStreamTransferNumberUrb)
            {
                //Set qty. of data transfer block
                emStatus = GXSetInt(g_hDevice[0], GX_DS_INT_STREAM_TRANSFER_NUMBER_URB, ACQ_TRANSFER_NUMBER_URB);
                GX_VERIFY_EXIT(emStatus);
            }

            //Set Balance White Mode : Continuous
            emStatus = GXSetEnum(g_hDevice[0], GX_ENUM_BALANCE_WHITE_AUTO, GX_BALANCE_WHITE_AUTO_ONCE);
            GX_VERIFY_EXIT(emStatus);

            //Allocate the memory for pixel format transform 
            g_pRGBImageBuf = new unsigned char[g_nPayloadSize * 3]; 
            g_pRaw8Image = new unsigned char[g_nPayloadSize];

            //Device start acquisition
            emStatus = GXStreamOn(g_hDevice[0]);
            if(emStatus != GX_STATUS_SUCCESS)
            {
                //Release the memory allocated
                if (g_pRaw8Image != NULL)
                {
                    delete[] g_pRaw8Image;
                    g_pRaw8Image = NULL;
                }
                if (g_pRGBImageBuf != NULL)
                {
                    delete[] g_pRGBImageBuf;
                    g_pRGBImageBuf = NULL;
                }
                GX_VERIFY_EXIT(emStatus);
            }

            timer_ = this->create_wall_timer(50ms, std::bind(&PublisherNode::timer_callback, this)); 
        }
    private:
       void timer_callback()
        {
        int nRet = pthread_create(&g_nAcquisitonThreadID, NULL, ProcGetImage, NULL);
        if(nRet != 0)
        {
            //Release the memory allocated
            UnPreForAcquisition();

            GXCloseDevice(g_hDevice[0]);
            g_hDevice[0] = NULL;
            GXCloseLib();

            printf("<Failed to create the acquisition thread, App Exit!>\n");
            exit(nRet);
        }
        pthread_join(g_nAcquisitonThreadID, NULL);
        //std::string* data = static_cast<std::string*>(return_value);
        if (return_value != NULL) {
            std::string* data = static_cast<std::string*>(return_value);
            //std::cout << "Thread returned data: " << *data << std::endl;
            // 创建一个String类型的消息对象
            auto msg = std_msgs::msg::String();   
            // 填充消息对象中的消息数据                                    
            msg.data = *data;
            // 发布话题消息                                                 
            //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str()); 
            // 统计丢帧数量
            cout << "\rall_frame: " << all_frame << "  " << "accept_frame: " << accept_frame << "  " << "lose_frame: " << lose_frame << flush; 
            // 输出日志信息，提示已经完成话题发布   
            publisher_->publish(msg);  
            delete data;
            return_value = NULL;
        }
        else{
            cout << 1 << endl;
        }

      
        }
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
        cv::VideoCapture capture_;
        GX_STATUS emStatus = GX_STATUS_SUCCESS;
        uint32_t ui32DeviceNum = 0;
        size_t nSize = 0;
        bool g_bColorFilter = false; 
        unsigned char* g_pRaw8Image = NULL;
        bool g_bAcquisitionFlag = false;

};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PublisherNode>());
    rclcpp::shutdown();
    return 0;
}   



void GetErrorString(GX_STATUS emErrorStatus)
{
    char *error_info = NULL;
    size_t size = 0;
    GX_STATUS emStatus = GX_STATUS_SUCCESS;
    
    emStatus = GXGetLastError(&emErrorStatus, NULL, &size);
    if(emStatus != GX_STATUS_SUCCESS)
    {
        printf("<Error when calling GXGetLastError>\n");
        return;
    }
    
    // Alloc error resources
    error_info = new char[size];
    if (error_info == NULL)
    {
        printf("<Failed to allocate memory>\n");
        return ;
    }
    
    // Get error description
    emStatus = GXGetLastError(&emErrorStatus, error_info, &size);
    if (emStatus != GX_STATUS_SUCCESS)
    {
        printf("<Error when calling GXGetLastError>\n");
    }
    else
    {
        printf("%s\n", error_info);
    }

    // Realease error resources
    if (error_info != NULL)
    {
        delete []error_info;
        error_info = NULL;
    }
}
std::string SavePPMFile(uint32_t ui32Width, uint32_t ui32Height)
{
    char szName[FILE_NAME_LEN] = {0};
    std::string filename;
    if(g_pRGBImageBuf != NULL)
    {
        FILE* phImageFile = NULL;
        auto now = std::chrono::system_clock::now();
        auto now_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(now);
        auto epoch = now_ms.time_since_epoch();
        auto value = std::chrono::duration_cast<std::chrono::milliseconds>(epoch);
        long duration = value.count();

        std::time_t now_c = std::chrono::system_clock::to_time_t(now);
        std::tm now_tm = *std::localtime(&now_c);

        std::stringstream ss;
        ss << "/root/pic/image_status_" << std::put_time(&now_tm, "%Y%m%d_%H%M%S") << "_" << (duration % 1000) << ".ppm";
        filename = ss.str();
        //cout << filename << endl;
        snprintf(szName, FILE_NAME_LEN, "%s", filename.c_str());

        phImageFile = fopen(szName, "wb");
        if (phImageFile == NULL)
        {
            printf("Create or Open %s failed!\n", szName);
        }
        //Save color image
        fprintf(phImageFile, "P6\n%u %u 255\n", ui32Width, ui32Height);
        fwrite(g_pRGBImageBuf, 1, g_nPayloadSize * 3, phImageFile);
 
        fclose(phImageFile);
        phImageFile = NULL;
        //printf("Save %s succeed!\n", szName);
    }
    else
    {
        printf("Save %s failed!\n", szName);
    } 
    return filename;
}

int PixelFormatConvert(PGX_FRAME_BUFFER pFrameBuffer)
{
    VxInt32 emDXStatus = DX_OK;

    // Convert RAW8 or RAW16 image to RGB24 image
    switch (pFrameBuffer->nPixelFormat)
    {
        case GX_PIXEL_FORMAT_BAYER_GR8:
        case GX_PIXEL_FORMAT_BAYER_RG8:
        case GX_PIXEL_FORMAT_BAYER_GB8:
        case GX_PIXEL_FORMAT_BAYER_BG8:
        {
            // Convert to the RGB image
            emDXStatus = DxRaw8toRGB24((unsigned char*)pFrameBuffer->pImgBuf, g_pRGBImageBuf, pFrameBuffer->nWidth, pFrameBuffer->nHeight,
                              RAW2RGB_NEIGHBOUR, DX_PIXEL_COLOR_FILTER(g_i64ColorFilter), false);
            if (emDXStatus != DX_OK)
            {
                printf("DxRaw8toRGB24 Failed, Error Code: %d\n", emDXStatus);
                return PIXFMT_CVT_FAIL;
            }
            break;
        }
        case GX_PIXEL_FORMAT_BAYER_GR10:
        case GX_PIXEL_FORMAT_BAYER_RG10:
        case GX_PIXEL_FORMAT_BAYER_GB10:
        case GX_PIXEL_FORMAT_BAYER_BG10:
        case GX_PIXEL_FORMAT_BAYER_GR12:
        case GX_PIXEL_FORMAT_BAYER_RG12:
        case GX_PIXEL_FORMAT_BAYER_GB12:
        case GX_PIXEL_FORMAT_BAYER_BG12:
        {
            // Convert to the Raw8 image
            emDXStatus = DxRaw16toRaw8((unsigned char*)pFrameBuffer->pImgBuf, g_pRaw8Image, pFrameBuffer->nWidth, pFrameBuffer->nHeight, DX_BIT_2_9);
            if (emDXStatus != DX_OK)
            {
                printf("DxRaw16toRaw8 Failed, Error Code: %d\n", emDXStatus);
                return PIXFMT_CVT_FAIL;
            }
            // Convert to the RGB24 image
            emDXStatus = DxRaw8toRGB24(g_pRaw8Image, g_pRGBImageBuf, pFrameBuffer->nWidth, pFrameBuffer->nHeight,
                              RAW2RGB_NEIGHBOUR, DX_PIXEL_COLOR_FILTER(g_i64ColorFilter), false);
            if (emDXStatus != DX_OK)
            {
                printf("DxRaw8toRGB24 Failed, Error Code: %d\n", emDXStatus);
                return PIXFMT_CVT_FAIL;
            }
            break;
        }
        default:
        {
            printf("Error : PixelFormat of this camera is not supported\n");
            return PIXFMT_CVT_FAIL;
        }
    }
    return PIXFMT_CVT_SUCCESS;
}

void *ProcGetImage(void* pParam)
{
    std::string filename; // 确保在这里声明
    GX_STATUS emStatus = GX_STATUS_SUCCESS;

    //Thread running flag setup
    g_bAcquisitionFlag = true;
    PGX_FRAME_BUFFER pFrameBuffer = NULL;

    time_t lInit;
    time_t lEnd;
    uint32_t ui32FrameCount = 0;

//    while(g_bAcquisitionFlag)
//    {
        if(!ui32FrameCount)
        {
            time(&lInit);
        }

        // Get a frame from Queue
        emStatus = GXDQBuf(g_hDevice[0], &pFrameBuffer, 1000);
        if(emStatus != GX_STATUS_SUCCESS)
        {
            if (emStatus == GX_STATUS_TIMEOUT)
            {
                //continue;
            }
            else
            {
                GetErrorString(emStatus);
                //break;
            }
        }

        if(pFrameBuffer->nStatus != GX_FRAME_STATUS_SUCCESS)
        {   
            all_frame++;
            lose_frame++;
            printf("<Abnormal Acquisition: Exception code: %d>\n", pFrameBuffer->nStatus);
        }
        else
        {
            all_frame++;
            accept_frame++;
            ui32FrameCount++;
            time (&lEnd);
            // Print acquisition info each second.
            if (lEnd - lInit >= 1)
            {  
		    //strftime(filename, sizeof(filename), "/home/camera-ros2/camera/src/pictures/%Y%m%d_%H%M%S_%S.png", now_tm
			//	);
		    //printf("%s\n", filename);
            //    printf("<Successful acquisition: FrameCount: %u Width: %d Height: %d FrameID: %lu>\n", 
            //        ui32FrameCount, pFrameBuffer->nWidth, pFrameBuffer->nHeight, pFrameBuffer->nFrameID);
                ui32FrameCount = 0;
            }

            //if (g_bSavePPMImage)
            {   
                int nRet = PixelFormatConvert(pFrameBuffer);
                if (nRet == PIXFMT_CVT_SUCCESS)
                {
                    filename = SavePPMFile(pFrameBuffer->nWidth, pFrameBuffer->nHeight);
                }
                else
                {
                    printf("PixelFormat Convert failed!\n");
                }
                g_bSavePPMImage = false;
            }
        }

        emStatus = GXQBuf(g_hDevice[0], pFrameBuffer);
        if(emStatus != GX_STATUS_SUCCESS)
        {
            GetErrorString(emStatus);
            //break;
        }  
    std::string* filename1 = new std::string(filename);
    return_value = filename1;
    return 0;

}

void UnPreForAcquisition()
{
    //Release resources
    if (g_pRaw8Image != NULL)
    {
        delete[] g_pRaw8Image;
        g_pRaw8Image = NULL;
    }
    if (g_pRGBImageBuf != NULL)
    {
        delete[] g_pRGBImageBuf;
        g_pRGBImageBuf = NULL;
    }

    return;
}
